#include <TimerOne.h>

// shared code
// ================================================
// BLDC 
// ================================================
#define INH_A  2
#define IN_A   3
#define INH_B  4
#define IN_B   5
#define INH_C  6
#define IN_C   7
#define HALL_A 8
#define HALL_B 10
#define HALL_C 11


const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};
const int HALL[] = {HALL_A, HALL_B, HALL_C};


void InitBLDC()
{
  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    digitalWrite(INH[i], LOW);
    pinMode(IN[i], OUTPUT);
    digitalWrite(IN[i], LOW);
    pinMode(HALL[i], INPUT_PULLUP);
  }
}


void BLDCSet_NoPWM(const byte inh[], const byte in[]){
  for (int i=0; i<3; i++){
    digitalWrite(INH[i], inh[i]);
    digitalWrite(IN[i], in[i]);
  }
}


// ================================================
// Try to switch the BLDC when any hall changes
// ================================================

int g_idx_step = 0;
// pin status on each step
byte steps[6][6] = {  // phase in INH_A, IN_A, INH_B, IN_B, INH_C, IN_C
  {1,1,1,0,0,0},  // AB
  {1,1,0,0,1,0},  // AC
  {0,0,1,1,1,0},  // BC
  {1,0,1,1,0,0},  // BA
  {1,0,0,0,1,1},  // CA
  {0,0,1,0,1,1}  // CB
};


void NextStep(byte inh[], byte in[]){
  g_idx_step++;
  if (g_idx_step >= 6){
    g_idx_step = 0;
  }

  // copy state of current
  for (int i=0; i<3; i++){
    inh[i] = steps[g_idx_step][2*i];
    in[i] = steps[g_idx_step][2*i+1];
  }
}


bool IsHallChanged(){
  static byte last_hall[3] = {0};
  for (int i=0; i<3; i++) {
    byte this_hall = digitalRead(HALL[i]);
    if (this_hall != last_hall[i]){
      // only one hall changes at once, so update the changed one is enough
      last_hall[i] = this_hall;
      return true;
    }
  }
  return false;
}


void PrintHallStep(const byte inh[], const byte in[]){
  Serial.print("step & hall: ");
  for (int i=0; i<3; i++){
    Serial.print(inh[i]); Serial.print(in[i]);
    Serial.print(" ");
  }
  
  for (int i=0; i<3; i++){
    Serial.print(digitalRead(HALL[i]));
  }
  Serial.println();
}


void setup() {
  Serial.begin(115200);
  pinMode(9, OUTPUT); digitalWrite(9, HIGH);
  Timer1.initialize(200);  // us
  Timer1.pwm(9, 30);  // 0 to 1023
  InitBLDC();
}


byte inh[3], in[3];
long lastTime = millis();
long thisTime = lastTime;
const unsigned int interval = 10;

void loop() {
  thisTime = millis();
  if ((thisTime - lastTime) > interval){
    lastTime = thisTime;
    NextStep(inh, in);
    BLDCSet_NoPWM(inh, in);
  }
  
  PrintHallStep(inh, in);
  delay(1);
}
